/*****************************************************
  Mars explorer NXT code in Ch
  University of Leicester GReP 2011
  Using http://iel.ucdavis.edu/projects/chmindstorms/
 *****************************************************/
/*****************************************************
  mapping.ch 
  
  Main mapping test code, the robot should map the
  surroundings, then move to the furthest visible
  point. The robot then rescans the area and repeats
  the process, aiming to built up a map of the area.
  
  Currently the code does not work in most situations,
  magnetic fields other than the Earth's interfere
  with the robot's bearings, causing the robot's
  location to be stored erroneously. To remedy this
  the code should be changed to assume that the motors
  can rotate the robot accurately, so therefore the
  magnetic compass will not be required.
 *****************************************************/
 
#include <conio.h>
#include <math.h>
#include <iostream>
#include <stdio.h>

#include "control.h"

#define MAXRANGE  250
#define PI 3.1415926535898
#define POINTS 40

//#define DEBUG

using namespace std;

struct position
{
  double R,theta; // the polar coords that define the current movement
  
  // After each move + turn the polar coords are added on to these
  double cxy[2];
  double dxy[2];
};

void d2r(double &angle)
{
  angle *= PI / 180.0;
}

double r2d(double &angle)
{
  angle *= 180.0 / PI;
  return angle;
}

void posupdate(double *pos1, double *pos2, double pos3, double pos4)
{
  *pos1 += pos3;
  *pos2 += pos4;
}

void cart2pol(double pos1, double pos2, double *r, double *theta)
{
 *r = -sqrt(pow(pos1,2) + pow(pos2,2));
 
if(pos1>=0 && pos2>=0){ 
  *theta = asin(pos1 / (*r));
}
 if(pos1>0 && pos2<0){
  *theta = (PI/2) + asin(pos1 / (*r));
 }
  if(pos1<0 && pos2<0){
  *theta = (PI) + asin(pos1 / (*r));
 }
  if(pos1<0 && pos2>0){
  *theta = (3/2 * (PI)) + asin(pos1 / (*r));
 } else{
 *theta = asin(pos1 / (*r));
}
 
//cout << "CART2POL " << *r << " " << *theta << endl;
}

void pol2cart(double *pos1, double *pos2, double theta, double r)
{
  *pos1 = cos(theta) * r;
  *pos2 = sin(theta) * r;
}

struct node // node is where the robot stops in x,y
{  
  double mappingx;
  double mappingy; // the data    
  node *next; // pointer to next node
};

node *start_ptr = NULL;
node *current;

void addroute (double *tx, double *ty)
{
  node *temp, *temp2; // temporary pointers
  
  // Reserve space for new node and fill it with data
  temp = new node; // node is where the robot stops in x,y
  temp->mappingx = *tx; // tx = temporary x
  temp->mappingy = *ty;
  temp->next = NULL;
  
  // Set up link to this node
  if (start_ptr == NULL)
  {
    start_ptr = temp;
  }
  else
  { 
    temp2 = start_ptr;
    // We know this is not NULL - list not empty!
    while (temp2->next != NULL)
    {
      // Move to next link in chain
      temp2 = temp2->next;
    }
    temp2->next = temp;
  }
}

struct node1
{  
  double mappingx;
  double mappingy; // the data    
  node1 *next; // pointer to next node
};

node1 *start_map = NULL;
node1 *current1;

void addmap (double tx, double ty)
{
  node1 *temp, *temp2; // Temporary pointers
  
  // Reserve space for new node and fill it with data
  temp = new node1;
  temp->mappingx = tx;
  temp->mappingy = ty;
  temp->next = NULL;
  
  // Set up link to this node
  if (start_map == NULL)
  {
    start_map = temp;
  }
  else
  { 
    temp2 = start_map;
    // We know this is not NULL - list not empty!
    while (temp2->next != NULL)
    {  
      // Move to next link in chain
      temp2 = temp2->next;
    }
    temp2->next = temp;
  }
}

void discheck(double pos1, double pos2, double pos3, double pos4, double *r, double *t)
{
  double xval, yval, rval, tval;
  xval =  pos1 - pos3;
  yval =  pos2 - pos4;
  //cout << "DISCHECK " << pos1 << " " << pos2 << " " << pos3 << " " << pos4 << endl;
  cart2pol(xval, yval, &rval, &tval);
  *r = fabs(rval);
  *t = fabs(tval);
}

void mkmap(Sensor &ultra, Sensor &compass, double *orT, double *orx, double *ory)
{
 // double theta = 0;
  double opsx, opsy, tmpx, tmpy;
  int numpoints = POINTS;
	double angle[numpoints]; //this stores the actual angle moved as reported from the tachometer
	double distance[numpoints]; //this stores the data received from the ultrasonic sensor
  int i;
  
  /*
  do
  {
    // get distance -> range
    range = 30;
    if(range < MAXRANGE)
    {
      tmpx = *orx;
      tmpy = *ory;
      angle = theta + *orT;
      d2r(angle);
      pol2cart(&opsx, &opsy, angle, range);
      posupdate(&tmpx, &tmpy, opsx, opsy);
      addmap(tmpx, tmpy);
      theta = theta + 4; // this is the rotation
    }
  } 
  while (theta < 360);
  */
  
  scanUltra(ultra, compass, numpoints, angle, distance);
  
  for (i=0; i<numpoints; i++) // copy readings to list
  {
    if(distance[i] < MAXRANGE)
    {
      tmpx = *orx;
      tmpy = *ory;
      cout << "XXXXXXXXXXTESTXXXXXXXXXXXXX" << endl;
      cout << tmpx << " " << tmpy << endl;
      d2r(angle[i]);
      pol2cart(&opsx, &opsy, angle[i], distance[i]);
      cout << opsx << " " << opsy << endl;
      posupdate(&tmpx, &tmpy, opsx, opsy);
      cout << tmpx << " " << tmpy << endl;
      addmap(tmpx, tmpy);
      //addmap(opsx, opsy);
    }
  }
}

int main()
{
  FILE *myfile;
  
  double bearing, distance, bearing1;
  int i;
  
  setup();
  Sensor ultra = Sensor(PORT_4, ULTRASONIC, RAWMODE);
  Sensor compass = Sensor(PORT_3, ULTRASONIC, RAWMODE);
  
  position robot;
  robot.dxy[0] = 0; // dx is difference in x position
  robot.dxy[1] = 0;
  robot.cxy[0] = 0; // cx is current x position
  robot.cxy[1] = 0;
  double newmove[2], ckmv[2]; // newmove is the calculated move to get to the furthest obstruction, ckmv = checkmove is for calculation purposes
  newmove[0] = 0;
  newmove[1] = 0;

  myfile = fopen("output/mapping.dat", "w"); // mapping.dat stores all data points from ultrasound to create a map
  
  //start_ptr = NULL; // start_ptr are the initial points in each list.
  //start_map = NULL;

  while (1) // this is a loop in which it constantly updates its known position from each move
  {    
    #ifdef DEBUG
      cout << "starting bearing" << endl;
    #endif
    bearing = 2 * compass.check(); // will get a compass reading
    bearing1 = bearing;
    d2r(bearing);
   
    #ifdef DEBUG
      cout << "first move" << endl;
    #endif
    
    cout << "new move " << newmove[1] << newmove [0] << endl;
    
    // Move robot
    rotateStep((r2d(newmove[1])), 10); // this is changed to make it rotate the bearing from where ever it is. aslo makes it zero its self
    moveStep((newmove[0] ), 20);
    rotateStep((-bearing1), 10);
    distance = newmove[0]; // gets a distance, from calibration or ultrasound
    
    pol2cart(&robot.dxy[0], &robot.dxy[1], bearing, distance);
    #ifdef DEBUG
      cout << "robot was at x,y = " << robot.cxy[0] << "," << robot.cxy[1] << endl;
      cout << "robot was at x,y = " << robot.dxy[0] << "," << robot.dxy[1] << endl;
    #endif
    posupdate(&robot.cxy[0], &robot.cxy[1], robot.dxy[0], robot.dxy[1]);
    #ifdef DEBUG
      cout << "robot is now at x,y = " << robot.cxy[0] << "," << robot.cxy[1] << endl;
    #endif
    
    //cart2pol(robot.dxy[0], robot.dxy[1], &bearing, &distance);
    
    #ifdef DEBUG
      cout << "to return, the robot takes a bearing of " << bearing << ", and moves " << distance << endl;
    #endif
    
    addroute(&robot.cxy[0], &robot.cxy[1]); // adding the stop location onto the list
    
    cout << "the map so far" << endl; // this is just reading out the list so far
    
    mkmap(ultra, compass, &bearing1, &robot.cxy[0], &robot.cxy[1]);
  
    node1 *temp1 = start_map;
    ckmv[0] = 0;
    ckmv[1] = 0;
    temp1 = temp1->next;

    do
    {
      if (temp1 == NULL)
      {
        cout << "End of list" << endl;
      }
      else // display details for what temp points to
      {  
        #ifdef DEBUG
          cout << "the XY are " << temp1->mappingx << " , " << temp1->mappingy << endl;
        #endif
        fprintf(myfile, "%f %f\n", temp1->mappingx, temp1->mappingy);
        discheck(robot.cxy[0], robot.cxy[1], temp1->mappingx, temp1->mappingy, &newmove[0], &newmove[1]);  
        
        if(newmove[0] > ckmv[0]) // move to next node (if present)
        {
          ckmv[0] = newmove[0];
          ckmv[1] = newmove[1];
        }
        
        temp1 = temp1->next;
      }
    }
    while (temp1 != NULL);
    
    newmove[0] = fabs(newmove[0] - 10); // to stop it hitting the wall
    
    if (key == 'e')
    {
      cout << "\ne key pressed, ending movement loop" << endl;
      break;
    }
  }
  
  cout << "the move is " << newmove[0] << " , " << newmove[1] << endl;
  
  fclose(myfile);
  
  system("wgnuplot output\\mapping.plt");
  
  node *temp2 = start_ptr;
  temp2 = temp2->next;
  
  myfile = fopen("output/route.dat", "w");
  
  do
  {  
    if (temp2 == NULL)
    {
      cout << "End of list" << endl;
    }
    else // display details for what temp points to
    {  
      #ifdef DEBUG
        cout << "the XY are " << temp2->mappingx << " , " << temp2->mappingy << endl;
      #endif
      fprintf(myfile, "%f %f\n", temp2->mappingx, temp2->mappingy);
      
      temp2 = temp2->next; // move to next node (if present)
    }
  }
  while (temp2 != NULL);
  
  fclose(myfile);
  
  system("wgnuplot output\\route.plt");
  
  system("pause");
}
